#include "liomath.h"

namespace Lio {

// Matrix math
BOOL Matrix::import(CSTR str, CSTR colSep, CSTR rowSep)
{
	int i,j;
	NumberFormat format;
	Tokenizer tokLines,tokElems;
	Tokenizer::Object line,elem;
	tokLines.set(str,rowSep);
	if(line=tokLines.first)
	{
		tokElems.set(line->value,colSep);
		if(elem=tokElems.first)
		{
			setSize(tokLines.length,tokElems.length);
			clear();
			for(i=0;;i++)
			{
				for(j=0;elem;j++,elem=elem->next)
					elems[i][j]=format.toDouble(elem->value);
				if(!line->next)
					break;
				line=line->next;
				tokElems.set(line->value,colSep);
				elem=tokElems.first;
			}
			return TRUE;
		}
	}
	return FALSE;
}
BOOL Matrix::diag(double *diagElems)
{
	if(!diagElems)
		return FALSE;
	clear();
	for(int i=0;i<rows&&i<cols;i++)
		elems[i][i]=diagElems[i];
	return TRUE;
}
Matrix Matrix::trans() const
{
	Matrix transM(cols,rows);
	for(int i=0;i<rows;i++)
		for(int j=0;j<cols;j++)
			transM.elems[j][i]=elems[i][j];
	return transM;
}
Matrix Matrix::diag() const
{
	int dim=min(cols,rows);
	Matrix diagM(dim,1);
	for(int i=0;i<dim;i++)
		diagM.elems[i][0]=elems[i][i];
	return diagM;
}
Matrix Matrix::inv(double threshold, BOOL pseudo) const
{
	if(cols==rows)
	{
		Matrix invM;
		if(!invM.setSize(cols,cols))
			throw MEM_ERROR;
		if(!invMatrix(elems,invM.elems,cols,threshold))
			throw MATH_ERROR;
		return invM;
	}
	else
	{
		if(!pseudo)
			throw MATH_ERROR;
		MatrixInvertor minv;
		if(!minv.bind(this))
			throw MEM_ERROR;
		if(!minv.inv(threshold))
			throw MATH_ERROR;
		return minv.mInvM;
	}
}

MATRIX MatrixInvertor::bind(CMATRIX M, int rows, int cols)
{
	if(	rows&&cols&&M&&
		(invM=mInvM.setSize(cols,rows))&&
		(Mt=mMt.setSize(cols,rows))&&
		(MtM=mMtM.setSize(cols,cols))&&
		(invMtM=mInvMtM.setSize(cols,cols))&&
		(invBuff=mInvBuff.setSize(cols,cols)))
	{
		this->M=M;
		this->cols=cols;
		this->rows=rows;
		return invM;
	}
	this->M=NULL;
	return NULL;
}
MATRIX MatrixInvertor::inv(double threshold)
{
	if(M)
	{
		transMatrix(M,Mt,rows,cols);
		mulMatrix(Mt,M,MtM,cols,rows,cols);
		if(invMatrix(MtM,invMtM,cols,threshold,invBuff))
		{
			mulMatrix(invMtM,Mt,invM,cols,cols,rows);
			return invM;
		}
	}
	return NULL;
}

BOOL importMatrix(	MATRIX m, int rows, int cols,
					const double * data,
					BOOL colrow)
{
	register int i,j;
	if(!m||!data)
		return FALSE;
	if(colrow)
		for(i=0;i<rows;i++)
			for(j=0;j<cols;j++)
				m[i][j]=data[i+j*rows];
	else
		for(i=0;i<rows;i++)
			for(j=0;j<cols;j++)
				m[i][j]=data[i*cols+j];
	return TRUE;
}
BOOL exportMatrix(	CMATRIX m, int rows, int cols,
					double *data,
					BOOL colrow)
{
	register int i,j;
	if(!m||!data)
		return FALSE;
	if(colrow)
		for(i=0;i<rows;i++)
			for(j=0;j<cols;j++)
				data[i+j*rows]=m[i][j];
	else
		for(i=0;i<rows;i++)
			for(j=0;j<cols;j++)
				data[i*cols+j]=m[i][j];
	return TRUE;
}
BOOL randMatrix(MATRIX m, int rows, int cols, double min, double max)
{
	int i,j;
	if(!m)
		return FALSE;
	for(i=0;i<rows;i++)
		for(j=0;j<cols;j++)
			m[i][j]=random(min,max);
	return TRUE;
}
void randMatrix(MATRIX3 m, double min, double max)
{
	m[0][0]=random(min,max);
	m[0][1]=random(min,max);
	m[0][2]=random(min,max);
	m[1][0]=random(min,max);
	m[1][1]=random(min,max);
	m[1][2]=random(min,max);
	m[2][0]=random(min,max);
	m[2][1]=random(min,max);
	m[2][2]=random(min,max);
}
void randMatrix(MATRIX2 m, double min, double max)
{
	m[0][0]=random(min,max);
	m[0][1]=random(min,max);
	m[1][0]=random(min,max);
	m[1][1]=random(min,max);
}
BOOL copyMatrix(CMATRIX m, MATRIX out, int rows, int cols)
{
	if(!m||!out)
		return FALSE;
	register int i;
	for(i=0;i<rows;i++)
		memcpy(out[i],m[i],sizeof(double)*cols);
	return TRUE;
}
BOOL oneMatrix(MATRIX m, int rows, int cols)
{
	if(!m)
		return FALSE;
	register int i;
	for(i=0;i<rows;i++)
	{
		ClearArray(m[i],cols);
		m[i][i]=1.0;
	}
	return TRUE;
}
void clearMatrix(MATRIX m, int rows, int cols)
{
	if(!m)return;
	register int i;
	for(i=0;i<rows;i++)
		ClearArray(m[i],cols);
}
BOOL mulMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows1, int cols1_rows2, int cols2, double coef)
{	/** out = coef*m1*m2 */
	register int i,j,k;
	double tmp;
	if(!m1||!m2||!out||m1==out||m2==out)
		return FALSE;
	for(i=0;i<rows1;i++)
	{
		for(j=0;j<cols2;j++)
		{
			tmp=m1[i][0]*m2[0][j];
			for(k=1;k<cols1_rows2;k++)
				tmp+=m1[i][k]*m2[k][j];
			out[i][j]=coef*tmp;
		}
	}
	return TRUE;
}
BOOL mulMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows1, int cols1_rows2, int cols2)
{	/** out = coef*m1*m2 */
	register int i,j,k;
	double tmp;
	if(!m1||!m2||!out||m1==out||m2==out)
		return FALSE;
	for(i=0;i<rows1;i++)
	{
		for(j=0;j<cols2;j++)
		{
			tmp=m1[i][0]*m2[0][j];
			for(k=1;k<cols1_rows2;k++)
				tmp+=m1[i][k]*m2[k][j];
			out[i][j]=tmp;
		}
	}
	return TRUE;
}
BOOL mulMatrix(CMATRIX m, double n, MATRIX out, int rows, int cols)
{
	register int i,j;
	if(!m||!out)
		return FALSE;
	for(i=0;i<rows;i++)
		for(j=0;j<cols;j++)
			out[i][j]=n*m[i][j];
	return TRUE;
}
BOOL addMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows, int cols)
{
	register int i,j;
	if(!m1||!m2||!out)
		return FALSE;
	for(i=0;i<rows;i++)
		for(j=0;j<cols;j++)
			out[i][j]=m1[i][j]+m2[i][j];
	return TRUE;
}
BOOL addMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows, int cols, double k1, double k2)
{
	register int i,j;
	if(!m1||!m2||!out)
		return FALSE;
	for(i=0;i<rows;i++)
		for(j=0;j<cols;j++)
			out[i][j]=k1*m1[i][j]+k2*m2[i][j];
	return TRUE;
}
double detMatrix(CMATRIX m, int dim, double threshold, MATRIX buffer)
{
	if(dim<=0)
		return 0;
	switch(dim)
	{
	    case 1:
		{
	    	return m[0][0];
		}
	    case 2:
		{
	    	return m[0][0]*m[1][1]-m[0][1]*m[1][0];
		}
	    case 3:
		{
	    	return 
	    		+m[0][0]*m[1][1]*m[2][2]
		    	+m[0][1]*m[1][2]*m[2][0]
		    	+m[1][0]*m[2][1]*m[0][2]
		    	-m[2][0]*m[1][1]*m[0][2]
		    	-m[2][1]*m[1][2]*m[0][0]
		    	-m[1][0]*m[0][1]*m[2][2];
		}
	    default:
		{
			Table<double> table;
			BOOL degen;
			register int k,col,row;
			double coef,det;

			if(!buffer)
			{
				if(!table.setSize(dim,dim))
					return FALSE;
				buffer=table.elems;
			}
			copyMatrix(m,buffer,dim,dim);
			k=0; // iteration index
			degen=FALSE;
			det=m[0][0];
			while(k<dim-1)
			{
				if(ABS(buffer[k][k])<=threshold)
				{
					degen=TRUE;
					for (row=k+1;row<dim;row++)
					{
						if(ABS(buffer[row][k])>threshold)
						{
							degen=FALSE;
							// Add Row 'row' To Row 'k'
							for(col=0;col<dim;col++)
								buffer[k][col]+=buffer[row][col];
							break;
						}
					}
					if(degen)
						return 0;
				}
				for (row=k+1;row<dim;row++)
				{
					coef=-buffer[row][k]/buffer[k][k];
					//Add Row 'k' Multiplied By 'coef' To Row 'row'
					for(col=k;col<dim;col++)
						buffer[row][col]+=coef*buffer[k][col];
				}
				k++;
				det*=buffer[k][k];
			}
			return det;
		}
	}
}
BOOL invMatrix(CMATRIX m, MATRIX out, int dim, double threshold, MATRIX buffer)
{
	if(!m||!out||dim<=0)
		return FALSE;
	switch(dim)
	{
		case 1:
		{
			if(ABS(m[0][0])<=threshold)
				return FALSE;
			out[0][0]=INV(m[0][0]);
			break;
		}
		case 2:
		{
			double det=m[0][0]*m[1][1]-m[0][1]*m[1][0];
			if(ABS(det)<=threshold)
				return FALSE;
			double den=INV(det);
			out[0][0]= m[1][1]*den;
			out[1][1]= m[0][0]*den;
			out[0][1]=-m[0][1]*den;
			out[1][0]=-m[1][0]*den;
			break;
		}
		case 3:
		{
			double det= m[0][0]*m[1][1]*m[2][2]
						+m[1][0]*m[2][1]*m[0][2]
						+m[2][0]*m[0][1]*m[1][2]
						-m[2][0]*m[1][1]*m[0][2]
						-m[1][0]*m[0][1]*m[2][2]
						-m[0][0]*m[2][1]*m[1][2];
			if(ABS(det)<=threshold)
				return FALSE;
			double den=INV(det);
			out[0][0]=(m[2][2]*m[1][1]-m[2][1]*m[1][2])*den;
			out[0][1]=(m[2][1]*m[0][2]-m[2][2]*m[0][1])*den;
			out[0][2]=(m[1][2]*m[0][1]-m[1][1]*m[0][2])*den;
			out[1][0]=(m[2][0]*m[1][2]-m[2][2]*m[1][0])*den;
			out[1][1]=(m[2][2]*m[0][0]-m[2][0]*m[0][2])*den;
			out[1][2]=(m[1][0]*m[0][2]-m[1][2]*m[0][0])*den;
			out[2][0]=(m[2][1]*m[1][0]-m[2][0]*m[1][1])*den;
			out[2][1]=(m[2][0]*m[0][1]-m[2][1]*m[0][0])*den;
			out[2][2]=(m[1][1]*m[0][0]-m[1][0]*m[0][1])*den;
			break;
		}
		default:
		{
			Table<double> table;
			register int i,j,k;
			double coef;
			BOOL degen=FALSE;

			if(!buffer)
			{
				if(!table.setSize(dim,dim))
					return FALSE;
				buffer=table.elems;
			}
			copyMatrix(m,buffer,dim,dim);
			oneMatrix(out,dim,dim);

			for (i = 0; i<dim-1; i++)
			{
				if(abs(buffer[i][i])<=threshold)
				{
					degen=TRUE;
					for (k = i+1; k<dim; k++)
					{
						if(abs(buffer[k][i])>threshold)
						{
							degen=FALSE;
							for (j = 0; j<dim; j++)
							{
								buffer[i][j]+=buffer[k][j];
								out[i][j]+=out[k][j];
							}
							break;
						}
					}
					if(degen)
						return FALSE;
				}
				coef=INV(buffer[i][i]);
				for (j = 0; j<dim; j++)
				{
					buffer[i][j]*=coef;
					out[i][j]*=coef;
				}
				for (k = i+1; k<dim; k++)
				{
					coef=-buffer[k][i];
					for (j = 0; j<dim; j++)
					{
						buffer[k][j]+=coef*buffer[i][j];
						out[k][j]+=coef*out[i][j];
					}
				}
			}
			if(abs(buffer[dim-1][dim-1])<=threshold)
				return FALSE;
			coef=INV(buffer[dim-1][dim-1]);
			for (j = 0; j<dim; j++)
			{
				buffer[dim-1][j]*=coef;
				out[dim-1][j]*=coef;
			}
			for (i = dim-1; i>0; i--)
			{
				for (k = i-1; k>=0; k--)
				{
					coef=-buffer[k][i];
					for (j = 0; j<dim; j++)
						out[k][j]+=coef*out[i][j];
				}
			}
		}
	}
	return TRUE;
}
static CMATRIX3 I3 = {
	{1, 0, 0},
	{0, 1, 0},
	{0, 0, 1}
};
void oneMatrix(MATRIX3 m)
{
	memcpy(m,I3,sizeof(MATRIX3));
}
void mulMatrix(CMATRIX3 m1, MATRIX3 m2, MATRIX3 out)
{
	MATRIX3 tmp;
	if(!out)out=m2;
	tmp[0][0]=m1[0][0]*m2[0][0]+m1[0][1]*m2[1][0]+m1[0][2]*m2[2][0];
	tmp[0][1]=m1[0][0]*m2[0][1]+m1[0][1]*m2[1][1]+m1[0][2]*m2[2][1];
	tmp[0][2]=m1[0][0]*m2[0][2]+m1[0][1]*m2[1][2]+m1[0][2]*m2[2][2];
	tmp[1][0]=m1[1][0]*m2[0][0]+m1[1][1]*m2[1][0]+m1[1][2]*m2[2][0];
	tmp[1][1]=m1[1][0]*m2[0][1]+m1[1][1]*m2[1][1]+m1[1][2]*m2[2][1];
	tmp[1][2]=m1[1][0]*m2[0][2]+m1[1][1]*m2[1][2]+m1[1][2]*m2[2][2];
	tmp[2][0]=m1[2][0]*m2[0][0]+m1[2][1]*m2[1][0]+m1[2][2]*m2[2][0];
	tmp[2][1]=m1[2][0]*m2[0][1]+m1[2][1]*m2[1][1]+m1[2][2]*m2[2][1];
	tmp[2][2]=m1[2][0]*m2[0][2]+m1[2][1]*m2[1][2]+m1[2][2]*m2[2][2];
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[0][2]=tmp[0][2];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
	out[1][2]=tmp[1][2];
	out[2][0]=tmp[2][0];
	out[2][1]=tmp[2][1];
	out[2][2]=tmp[2][2];
}
void mulMatrix(CMATRIX2 m1, MATRIX2 m2, MATRIX2 out)
{
	MATRIX2 tmp;
	if(!out)out=m2;
	tmp[0][0]=m1[0][0]*m2[0][0]+m1[0][1]*m2[1][0];
	tmp[0][1]=m1[0][0]*m2[0][1]+m1[0][1]*m2[1][1];
	tmp[1][0]=m1[1][0]*m2[0][0]+m1[1][1]*m2[1][0];
	tmp[1][1]=m1[1][0]*m2[0][1]+m1[1][1]*m2[1][1];
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
}
void mulMatrix(MATRIX3 m, double n, MATRIX3 out)
{
	if(!out)
		out=m;
	out[0][0]=(n*m[0][0]);
	out[0][1]=(n*m[0][1]);
	out[0][2]=(n*m[0][2]);
	out[1][0]=(n*m[1][0]);
	out[1][1]=(n*m[1][1]);
	out[1][2]=(n*m[1][2]);
	out[2][0]=(n*m[2][0]);
	out[2][1]=(n*m[2][1]);
	out[2][2]=(n*m[2][2]);
}
void addMatrix(CMATRIX3 m1, MATRIX3 m2, MATRIX3 out)
{
	if(!out)
		out=m2;
	out[0][0]=m1[0][0]+m2[0][0];
	out[0][1]=m1[0][1]+m2[0][1];
	out[0][2]=m1[0][2]+m2[0][2];
	out[1][0]=m1[1][0]+m2[1][0];
	out[1][1]=m1[1][1]+m2[1][1];
	out[1][2]=m1[1][2]+m2[1][2];
	out[2][0]=m1[2][0]+m2[2][0];
	out[2][1]=m1[2][1]+m2[2][1];
	out[2][2]=m1[2][2]+m2[2][2];
}
void addMatrix(CMATRIX3 m1, MATRIX3 m2, double k1, double k2, MATRIX3 out)
{
	if(!out)out=m2;
	out[0][0]=(k1*m1[0][0]+k2*m2[0][0]);
	out[0][1]=(k1*m1[0][1]+k2*m2[0][1]);
	out[0][2]=(k1*m1[0][2]+k2*m2[0][2]);
	out[1][0]=(k1*m1[1][0]+k2*m2[1][0]);
	out[1][1]=(k1*m1[1][1]+k2*m2[1][1]);
	out[1][2]=(k1*m1[1][2]+k2*m2[1][2]);
	out[2][0]=(k1*m1[2][0]+k2*m2[2][0]);
	out[2][1]=(k1*m1[2][1]+k2*m2[2][1]);
	out[2][2]=(k1*m1[2][2]+k2*m2[2][2]);
}
void addMatrix(CMATRIX2 m1, MATRIX2 m2, MATRIX2 out)
{
	if(!out)out=m2;
	out[0][0]=m1[0][0]+m2[0][0];
	out[0][1]=m1[0][1]+m2[0][1];
	out[1][0]=m1[1][0]+m2[1][0];
	out[1][1]=m1[1][1]+m2[1][1];
}
BOOL mulMatrixTrans(CMATRIX2 m1, CMATRIX2 m2, MATRIX2 out)
{	/** out = m1*m2*(m1') */
	double tmp;
	if(!m1||!m2||!out||m1==out||m2==out)
		return FALSE;
	tmp=m1[0][0]*m2[0][0]+m1[0][1]*m2[1][0];
	out[0][0]=m1[0][0]*tmp;
	out[0][1]=m1[1][0]*tmp;
	tmp=m1[0][0]*m2[0][1]+m1[0][1]*m2[1][1];
	out[0][0]+=m1[0][1]*tmp;
	out[0][1]+=m1[1][1]*tmp;
	tmp=m1[1][0]*m2[0][0]+m1[1][1]*m2[1][0];
	out[1][0]=m1[0][0]*tmp;
	out[1][1]=m1[1][0]*tmp;
	tmp=m1[1][0]*m2[0][1]+m1[1][1]*m2[1][1];
	out[1][0]+=m1[0][1]*tmp;
	out[1][1]+=m1[1][1]*tmp;
	return TRUE;
}
BOOL mulMatrixTrans(CMATRIX m1, CMATRIX m2, MATRIX out, int rows1, int cols1)
{	/** out = m1*m2*(m1') */
	register int i,j,k,l;
	double tmp;
	if(!m1||!m2||!out||m1==out||m2==out)
		return FALSE;
	for(i=0;i<rows1;i++)
	{
		ClearArray(out[i],rows1);
		for(j=0;j<cols1;j++)
		{
			tmp=m1[i][0]*m2[0][j];
			for(k=1;k<cols1;k++)
				tmp+=m1[i][k]*m2[k][j];
			for(l=0;l<rows1;l++)
				out[i][l]+=m1[l][j]*tmp;
		}
		//for(l=0;l<i;l++)
		//	out[i][l]=out[l][i];
	}
	return TRUE;
}
double detMatrix(CMATRIX3 m)
{
	return	 m[0][0]*m[1][1]*m[2][2]+m[1][0]*m[2][1]*m[0][2]+m[2][0]*m[0][1]*m[1][2]
			-m[2][0]*m[1][1]*m[0][2]-m[1][0]*m[0][1]*m[2][2]-m[0][0]*m[2][1]*m[1][2];
}
double detMatrix(CMATRIX2 m)
{
	return m[0][0]*m[1][1]-m[1][0]*m[0][1];
}
BOOL invMatrix(MATRIX3 m, MATRIX3 out)
{
	MATRIX3 tmp;
	if(!out)out=m;
	double det=detMatrix(m);
	if(det==0)return FALSE;
	double den=INV(det);
	tmp[0][0]=(m[2][2]*m[1][1]-m[2][1]*m[1][2])*den;
	tmp[0][1]=(m[2][1]*m[0][2]-m[2][2]*m[0][1])*den;
	tmp[0][2]=(m[1][2]*m[0][1]-m[1][1]*m[0][2])*den;
	tmp[1][0]=(m[2][0]*m[1][2]-m[2][2]*m[1][0])*den;
	tmp[1][1]=(m[2][2]*m[0][0]-m[2][0]*m[0][2])*den;
	tmp[1][2]=(m[1][0]*m[0][2]-m[1][2]*m[0][0])*den;
	tmp[2][0]=(m[2][1]*m[1][0]-m[2][0]*m[1][1])*den;
	tmp[2][1]=(m[2][0]*m[0][1]-m[2][1]*m[0][0])*den;
	tmp[2][2]=(m[1][1]*m[0][0]-m[1][0]*m[0][1])*den;
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[0][2]=tmp[0][2];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
	out[1][2]=tmp[1][2];
	out[2][0]=tmp[2][0];
	out[2][1]=tmp[2][1];
	out[2][2]=tmp[2][2];
	return TRUE;
}
BOOL invMatrix(MATRIX2 m, MATRIX2 out)
{
	MATRIX2 tmp;
	if(!out)out=m;
	double det=m[0][0]*m[1][1]-m[1][0]*m[0][1];
	if(det==0)return FALSE;
	double den=INV(det);
	tmp[0][0]=m[1][1]*den;
	tmp[0][1]=-m[0][1]*den;
	tmp[1][0]=-m[1][0]*den;
	tmp[1][1]=m[0][0]*den;
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
	return TRUE;
}
void transMatrix(MATRIX3 m, MATRIX3 out)
{
	MATRIX3 tmp;
	if(!out)out=m;
	tmp[0][0]=m[0][0];
	tmp[0][1]=m[1][0];
	tmp[0][2]=m[2][0];
	tmp[1][0]=m[0][1];
	tmp[1][1]=m[1][1];
	tmp[1][2]=m[2][1];
	tmp[2][0]=m[0][2];
	tmp[2][1]=m[1][2];
	tmp[2][2]=m[2][2];
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[0][2]=tmp[0][2];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
	out[1][2]=tmp[1][2];
	out[2][0]=tmp[2][0];
	out[2][1]=tmp[2][1];
	out[2][2]=tmp[2][2];
}
void transMatrix(MATRIX2 m, MATRIX2 out)
{
	MATRIX2 tmp;
	if(!out)out=m;
	tmp[0][0]=m[0][0];
	tmp[0][1]=m[1][0];
	tmp[1][0]=m[0][1];
	tmp[1][1]=m[1][1];
	out[0][0]=tmp[0][0];
	out[0][1]=tmp[0][1];
	out[1][0]=tmp[1][0];
	out[1][1]=tmp[1][1];
}
double traceMatrix(CMATRIX3 m)
{
	return m[0][0]+m[1][1]+m[2][2];
}
BOOL transMatrix(CMATRIX m, MATRIX out, int rows, int cols)
{
	if(!out||!m)
		return FALSE;
	register int i,j;
	for(i=0;i<rows;i++)
		for(j=0;j<cols;j++)
			out[j][i]=m[i][j];
	return TRUE;
}
double traceMatrix(CMATRIX m, int rows, int cols)
{
	if(!m)
		return 0;
	double result;
	register int i;
	for(i=1,result=m[0][0];i<rows&&i<cols;i++)
		result+=m[i][i];
	return result;
}
void printMatrix(FILE *file, CMATRIX m, int rows, int cols, CSTR start, CSTR line, CSTR format, CSTR end)
{
	if(!file)
		return;
	if(format==NULL)
		format=_T("%f ");
	if(start)
		_ftprintf(file,start);
	if(!line)
		line=_T("\n");
	if(rows&&cols)
	{
		for(int i=0;i<rows-1;i++)
		{
			for(int j=0;j<cols;j++)
				_ftprintf(file,format,m[i][j]);
			_ftprintf(file, line);
		}
		for(int j=0;j<cols;j++)
			_ftprintf(file,format,m[rows-1][j]);
	}
	if(end)
		_ftprintf(file,end);
}
void printMatrix(FILE *file, CMATRIX3 m, CSTR start, CSTR line, CSTR format, CSTR end)
{
	if(format==NULL)format=_T("%f ");
	if(start)_ftprintf(file,start);
	if(!line)line=_T("\n");
	_ftprintf(file,format,m[0][0]);
	_ftprintf(file,format,m[0][1]);
	_ftprintf(file,format,m[0][2]);
	_ftprintf(file, line);
	_ftprintf(file,format,m[1][0]);
	_ftprintf(file,format,m[1][1]);
	_ftprintf(file,format,m[1][2]);
	_ftprintf(file, line);
	_ftprintf(file,format,m[2][0]);
	_ftprintf(file,format,m[2][1]);
	_ftprintf(file,format,m[2][2]);
	if(end)_ftprintf(file,end);
}
void printMatrix(FILE *file, CMATRIX2 m, CSTR start, CSTR line, CSTR format, CSTR end)
{
	if(format==NULL)format=_T("%f ");
	if(start)_ftprintf(file,start);
	if(!line)line=_T("\n");
	_ftprintf(file,format,m[0][0]);
	_ftprintf(file,format,m[0][1]);
	_ftprintf(file, line);
	_ftprintf(file,format,m[1][0]);
	_ftprintf(file,format,m[1][1]);
	if(end)_ftprintf(file,end);
}

void importMatrix(MATRIX3 m, const double data[9], BOOL colrow)
{
	if(colrow)
	{
		m[0][0]=data[0];
		m[1][0]=data[1];
		m[2][0]=data[2];
		m[0][1]=data[3];
		m[1][1]=data[4];
		m[2][1]=data[5];
		m[0][2]=data[6];
		m[1][2]=data[7];
		m[2][2]=data[8];
	}
	else
	{
		m[0][0]=data[0];
		m[0][1]=data[1];
		m[0][2]=data[2];
		m[1][0]=data[3];
		m[1][1]=data[4];
		m[1][2]=data[5];
		m[2][0]=data[6];
		m[2][1]=data[7];
		m[2][2]=data[8];
	}
}
void exportMatrix(CMATRIX3 m, double data[9], BOOL colrow)
{
	if(colrow)
	{
		data[0]=m[0][0];
		data[1]=m[1][0];
		data[2]=m[2][0];
		data[3]=m[0][1];
		data[4]=m[1][1];
		data[5]=m[2][1];
		data[6]=m[0][2];
		data[7]=m[1][2];
		data[8]=m[2][2];
	}
	else
	{
		data[0]=m[0][0];
		data[1]=m[0][1];
		data[2]=m[0][2];
		data[3]=m[1][0];
		data[4]=m[1][1];
		data[5]=m[1][2];
		data[6]=m[2][0];
		data[7]=m[2][1];
		data[8]=m[2][2];
	}
}

void initQuat(QUAT q, double w, double x, double y, double z)
{
	q[0]=w;
	q[1]=x;
	q[2]=y;
	q[3]=z;
}
void initQuat(QUAT q, CQUAT q0)
{
	if(q0)
	{
		q[0]=q0[0];
		q[1]=q0[1];
		q[2]=q0[2];
		q[3]=q0[3];
	}
	else
	{
		q[0]=1;
		q[1]=0;
		q[2]=0;
		q[3]=0;
	}
}
void mulQuat(CQUAT q1, QUAT q2, QUAT out)
{
	if(!out)out=q2;
	QUAT tmp;
	tmp[0]=q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
	tmp[1]=q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
	tmp[2]=q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
	tmp[3]=q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
	out[0]=tmp[0];
	out[1]=tmp[1];
	out[2]=tmp[2];
	out[3]=tmp[3];
}
void mulQuat(QUAT q, double n, QUAT out)
{
	if(!out)out=q;
	out[0]=n*q[0];
	out[1]=n*q[1];
	out[2]=n*q[2];
	out[3]=n*q[3];
}
void addQuat(QUAT q1, QUAT q2, QUAT out)
{
	if(!out)out=q2;
	out[0]=q1[0]+q2[0];
	out[1]=q1[1]+q2[1];
	out[2]=q1[2]+q2[2];
	out[3]=q1[3]+q2[3];
}
void conjQuat(QUAT q, QUAT out)
{
	if(!out)out=q;
	out[0]=q[0];
	out[1]=-q[1];
	out[2]=-q[2];
	out[3]=-q[3];
}
void invQuat(QUAT q, QUAT out)
{
	if(!out)out=q;
	double tmp=q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3];
	tmp=tmp?INV(tmp):0;
	out[0]= q[0]*tmp;
	out[1]=-q[1]*tmp;
	out[2]=-q[2]*tmp;
	out[3]=-q[3]*tmp;
}
void normQuat(QUAT q, QUAT out)
{
	if(!out)out=q;
	double tmp=sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
	tmp=tmp?INV(tmp):0;
	out[0]=q[0]*tmp;
	out[1]=q[1]*tmp;
	out[2]=q[2]*tmp;
	out[3]=q[3]*tmp;
}
double getQuatNorm(QUAT q)
{
	return sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
}
double getQuatUnit(QUAT q, double unit[3])
{
	double tmp=q[1]*q[1]+q[2]*q[2]+q[3]*q[3];
	if(unit&&tmp>0)
	{
		tmp=INV(sqrt(tmp));
		unit[0]=q[1]*tmp;
		unit[1]=q[2]*tmp;
		unit[2]=q[3]*tmp;
	}
	return 2*acos(q[0]);
}
void printQuat(FILE *file, QUAT q, STR format, STR start, STR end)
{
	if(format==NULL)
		format=_T("%f ");
	if(start)
		_ftprintf(file,start);
	for(int i=0;i<4;i++)
		_ftprintf(file,format,q[i]);
	if(end)
		_ftprintf(file,end);
}
// Runge-Cutta Order Four
// Return Ending 1 Value By Runge-Cutta Order 4 Integration Of f 
double RungeCutta4(	double xEnd, // Ending Arg Value
					double xStart, double yStart, // Starting Point2d: (xStart,yStart)
					int n, // Number Of Steps
					double (*f)(double, double), // Function Derivate: dy/dx=f(xEnd,y)
					double *data) // If Present, data Must Have At Least n+1 Points !!!!!!!
{
	register int i;
	double dx=(xEnd-xStart)/n;
	double x=xStart,y=yStart,k1,k2,k3,k4;
	if(data==NULL)
	{
		for(i=0;i<n;i++)
		{
			k1=dx*f(x, y);
			k2=dx*f(x+dx*0.5, y+k1*0.5);
			k3=dx*f(x+dx*0.5, y+k2*0.5);
			k4=dx*f(x+dx, y+k3);
			y+=(k1+2*(k2+k3)+k4)/6;
			x+=dx;
		}
	}
	else
	{
		*data=y;
		for(i=0;i<n;i++)
		{
			k1=dx*f(x, y);
			k2=dx*f(x+dx*0.5, y+k1*0.5);
			k3=dx*f(x+dx*0.5, y+k2*0.5);
			k4=dx*f(x+dx, y+k3);
			y+=(k1+2*(k2+k3)+k4)/6;
			x+=dx;
			data++;
			*data=y;
		}
	}
	return y;
}
BOOL addNoise(double *data, int length, double stdev, double mean)
{
	if(!data||length<=0||stdev==0)
		return FALSE;
	for(register int i=0;i<length;i++)
		data[i]+=random_n(stdev,mean);
	return TRUE;
}
inline static double box_muller_loop(double *n1 = NULL, double *n2 = NULL)
{
	double rand1,rand2,l;
	do
	{
		rand1=2*random()-1;
		rand2=2*random()-1;
		l=rand1*rand1+rand2*rand2;
	}
	while(l>=1);
	if(n1)*n1=rand1;
	if(n2)*n2=rand2;
	return -2*log(l)/l;
}
double random_n(double stdev, double mean)
{
	double rand,l;
	l=box_muller_loop(&rand);
	return mean+rand*sqrt(l)*stdev;
}
double random_nv(double var, double mean)
{
	double rand,l;
	l=box_muller_loop(&rand);
	return mean+rand*sqrt(l*var);
}
void random_n(	double *n1, double *n2,	double correl,
				double stdev1, double stdev2,
				double mean1, double mean2)
{// Normal Distribution, Box-Muller method (Polar method)
	double rand1,rand2,l;
	l=sqrt(box_muller_loop(&rand1,&rand2));
	*n1=mean1+stdev1*l*rand1;
	*n2=mean2+stdev2*l*(correl*rand1+sqrt(1-correl*correl)*rand2);
}
BOOL random_n3d(double *n, const double *correl, const double *stdev, const double *mean)
{
	double tmp,l,r[3],c[3]={0,0,0},s[3]={1,1,1},m[3]={0,0,0};
	
	if(correl)
	{
		memcpy(c,correl,sizeof(c));
		if(c[0]*c[0]+c[1]*c[1]+c[2]*c[2]>1+2*c[0]*c[1]*c[2])
			return FALSE;
	}
	if(mean)
		memcpy(m,mean,sizeof(m));
	if(stdev)
		memcpy(s,stdev,sizeof(s));

	l=sqrt(box_muller_loop(r,r+1));
	r[0]*=l;
	r[1]*=l;
	l=box_muller_loop(r+2);

	tmp=sqrt(1-c[0]*c[0]);
	n[0]=m[0]+s[0]*r[0];
	n[1]=m[1]+s[1]*(c[0]*r[0]+tmp*r[1]);
	tmp=(c[1]-c[0]*c[2])/tmp;
	n[2]=m[2]+s[2]*(c[2]*r[0]+tmp*r[1]+sqrt((1-c[2]*c[2]-tmp*tmp)*l)*r[2]);
	return TRUE;
}
double mean(const double *values,int count)
{
	if(count<=0||!values)
		return 0;
	double tmp=0;
	for(register int i=0;i<count;i++)
		tmp+=values[i];
	return tmp/count;
}
double var(const double *values,int count)
{
	if(count<=0||!values)
		return 0;
	double tmp=0,tmp2=0;
	for(register int i=0;i<count;i++)
	{
		tmp+=values[i];
		tmp2+=values[i]*values[i];
	}
	return tmp2/count-pow2(tmp/count);
}
double var(const double *values,int count,double mean)
{
	if(count<=0||!values)
		return 0;
	double tmp2=0;
	for(register int i=0;i<count;i++)
		tmp2+=values[i]*values[i];
	return tmp2/count-mean*mean;
}
double avar(const double *values,int count)
{
	if(count<=0)
		return 0;
	double tmp=0;
	for(register int i=0;i<count-1;i++)
		tmp+=pow2(values[i+1]-values[i]);
	return tmp/(count<<1);
}
double covar(const double *values1,const double *values2,int count)
{
	if(count<=0)
		return 0;
	double tmp1=0,tmp2=0,tmp12=0;
	for(register int i=0;i<count;i++)
	{
		tmp12+=values1[i]*values2[i];
		tmp1+=values1[i];
		tmp2+=values2[i];
	}
	return tmp12/count-(tmp1/count)*(tmp2/count);
}
double correl(const double *values1,const double *values2,int count)
{
	if(count<=0)
		return 0;
	double m1=0,m2=0,v1=0,v2=0,v12=0;
	for(register int i=0;i<count;i++)
	{
		v12+=values1[i]*values2[i];
		v1+=values1[i]*values1[i];
		v2+=values2[i]*values2[i];
		m1+=values1[i];
		m2+=values2[i];
	}
	return (v12-(m1*m2)/count)/sqrt((v1-(m1*m1)/count)*(v2-(m2*m2)/count));
}
double conv(const double *data1, const double *data2, int count)
{
	double result=0;
	for(register int i=0;i<count;i++)
		result+=data1[i]*data2[i];
	return result;
}
double autocorrel(const double *values,int count,int shift)
{
	if(count<=0)
		return 0;
	double tmp=0;
	const double *values2=values+shift;
	for(register int i=0;i<count;i++)
		tmp+=values[i]*values2[i];
	return tmp/count;
}
double autocorrel(const double *values,int count,int shift,int index)
{
	if(index+shift<0||index+shift>=count)
		return 0;
	register int i;
	const double
		*v1=values+index,
		*v2=values+index+shift,
		*vn=values+count-1;
	double tmp=0;
	for(i=0;i<count;i++)
	{
		tmp+=(*v1)*(*v2);
		if(v1==vn)v1=values;
		else v1++;
		if(v2==vn)v2=values;
		else v2++;
	}
	return tmp/count;
}
double normalDistrib(double x, double precision)
{ // Normal Distribution With 0 Mean And 1 Standart Deviation
	if(x<-7)
		return 0;
	if(x>7)
		return 1;
	if(precision<=0)
		precision = 1e-7;
	int i=1;
	double y,arg,tmp;
	y=arg=x;
	do
	{
		arg*=x*x/(i*2);
		y+=tmp=sign1(i)*arg/(i*2+1);
		i++;
	}
	while(ABS(tmp)>precision);
	//_tprintf(_T("loops=%d\n"),i);
	return 0.5+y/(SQRT2*SQRTPI);
}
BOOL getDFT(const double *data, double *dft, int count)
{
	int k,n,j;
	double c,s,tmp;

	if(count<1||!data||!dft)
		return FALSE;

	c=data[0];
	for(k=1;k<count;k++)
		c+=data[k];
	dft[0]=c/count;
	for(n=1,j=count-1;n<=j;n++,j--)
	{
		tmp=(2*PI*n)/count;
		c=data[0];
		s=0;
		for(k=1;k<count;k++)
		{
			c+=data[k]*cos(tmp*k);
			s-=data[k]*sin(tmp*k);
		}
		dft[j]=dft[n]=sqrt(s*s+c*c)/count;
	}
	return TRUE;
}
BOOL getDFT(const double *data, double *dft_Re, double *dft_Im, int count)
{
	int k,n,j;
	double c,s,tmp;

	if(count<1||!data||!dft_Re||!dft_Im)
		return FALSE;

	c=data[0];
	for(k=1;k<count;k++)
		c+=data[k];
	double coef=INV(count);
	dft_Re[0]=coef*c;
	dft_Im[0]=0;
	for(n=1,j=count-1;n<=j;n++,j--)
	{
		tmp=coef*(2*PI*n);
		c=data[0];
		s=0;
		for(k=1;k<count;k++)
		{
			c+=data[k]*cos(tmp*k);
			s-=data[k]*sin(tmp*k);
		}
		dft_Re[j]=(dft_Re[n]=coef*c);
		dft_Im[j]=-(dft_Im[n]=coef*s);
	}
	return TRUE;
}
BOOL getIDFT(const double *dft_Re, const double *dft_Im, double *idft, int count)
{
	int k,n;
	double c,tmp;

	if(count<1||!idft||!dft_Re||!dft_Im)
		return FALSE;
	double coef=INV(count);
	c=dft_Re[0];
	for(n=1;n<count;n++)
		c+= dft_Re[n];
	idft[0]=c;
	for(k=1;k<count;k++)
	{
		tmp=coef*(2*PI*k);
		c=dft_Re[0];
		for(n=1;n<count;n++)
			c+= dft_Re[n]*cos(tmp*n)-dft_Im[n]*sin(tmp*n);
		idft[k]=c;
	}
	return TRUE;
}
int getAutoCorrelFunc(	const double *data, // data samples
						int count, // length of the data samples
						double *R, // auto correlation function data
						int shift, // max shift for auto-correlation calculation
						int index, // start index for auto-correlation calculation
						int scan) // scan length for auto-correlation calculation
{
	if(!data||count<=0||shift==0)
		return 0;
	if(shift>0)
	{
		if(index<0)
			index=0;
		if(scan<=0||index+shift+scan>count)
			scan=count-index-shift;
		if(scan<=0)
			return 0;
		for(int i=0,j=0; j<shift ;i++,j++)
			R[i]=autocorrel(data+index,scan,j);
	}
	else
	{
		if(index-shift<0)
			index=shift;
		if(scan<=0||index+scan>count)
			scan=count-index;
		if(scan<=0)
			return 0;
		for(int i=0,j=0; i>shift ;i++,j--)
			R[i]=autocorrel(data+index,scan,j);
	}
	return scan;
}

Stat::Stat()
{
	clear();
}
Stat::Stat(const Stat& stat)
{
	clear();
	mean=stat.mean;
	var=stat.mean;
	stdev=stat.stdev;
	skew=stat.skew;
	excess=stat.excess;
}
Stat::Stat(const double *values, int count)
{
	clear();
	add(values,count);
}
void Stat::clear()
{
	x=x2=x3=x4=0;
	mean=var=stdev=skew=excess=0;
	n=0;
}
void Stat::add(double value)
{
	x+=value;
	x2+=value*value;
	x3+=value*value*value;
	x4+=value*value*value*value;
	n++;
}
BOOL Stat::add(const double *values, int count)
{
	double value;
	if(!values||count<0)
		return FALSE;
	for(int i=0;i<count;i++)
	{
		value=values[i];
		x+=value;
		x2+=value*value;
		x3+=value*value*value;
		x4+=value*value*value*value;
	}
	n+=count;
	return TRUE;
}
BOOL Stat::update()
{
	mean=var=stdev=skew=excess=0;
	if(n)
	{
		double divn=INV(n);
		mean=x*divn;
		var=x2*divn-mean*mean;
		stdev=sqrt(var);
		if(stdev)
		{
			double divs = INV(stdev);
			skew=((x3-3*x2*mean)*divn+2*mean*mean*mean)*divs*divs*divs;
			excess=((x4-4*x3*mean+6*x2*mean*mean)*divn-3*mean*mean*mean*mean)*divs*divs*divs*divs-3;
		}
	}
	return n;
}
const double firWinCoefs[FIR_WIN_COUNT]={0.95,4.15,3.30,3.44,6.80,6.00,5.80,6.80,6.80};
int getFIROrder(	int windowType, // Type of the Truncation Window: FIE_WIN_TYPES
					double skirtLength, // Difference between passBand and stopBand
					double sampFreq, // Sample rate frequency
					BOOL forceOdd) // Set to TRUE if only odd number should be produced
{
	if(skirtLength<=0||sampFreq<skirtLength)
		return 0;
	if(windowType<0||windowType>=FIR_WIN_COUNT)
		windowType=FIR_WIN_RECT;
	int result=1+__int(firWinCoefs[windowType]*(sampFreq-skirtLength)/skirtLength);
	if(forceOdd&&result%2==0)
		result++;
	return result;
}
double getFIRSkirt(	int windowType, // Type of the Truncation Window: FIE_WIN_TYPES
					int order, // Filter order
					double sampFreq) // Sample rate frequence
{
	if(order<=0||sampFreq<=0)
		return 0;
	if(windowType<0||windowType>=FIR_WIN_COUNT)
		windowType=FIR_WIN_RECT;
	return sampFreq/(1+order/firWinCoefs[windowType]);
}
BOOL getFIRFilter(	double *fir, // Filter coefficients
					int order, // Filter coefficients count, must be odd number
					int filterType, // Filter type: LOWPASS, HIGHPASS, BANDPASS, BANDSTOP
					int windowType, // Type of the Truncation Window: FIE_WIN_TYPES
					double sampFreq, // Sample rate frequency
					double cutOffFreq1, // CutOff Frequency
					double cutOffFreq2) // Second cuttOff frequency in the case of BANDPASS and BANDSTOP, must be superior to cutOffFreq1
{
	int center,n;
	double Wc1,Wc2;
	if(!fir||order<=0||order%2==0||cutOffFreq1<0||cutOffFreq2<0||cutOffFreq1>sampFreq||cutOffFreq2>sampFreq)
		return FALSE;
	center=(order-1)/2;
	Wc1=cutOffFreq1/sampFreq;
	Wc2=cutOffFreq2/sampFreq;
	switch(filterType)
	{
		case HIGHPASS:
			fir[center]=1-Wc1*INVPI;
			for(n=1;n<=center;n++)
				fir[center+n]=-sin(Wc1*n)/(PI*n);
		break;
		case BANDPASS:
			fir[center]=(Wc2-Wc1)*INVPI;
			for(n=1;n<=center;n++)
				fir[center+n]=(sin(Wc2*n)-sin(Wc1*n))/(PI*n);
		break;
		case BANDSTOP:
			fir[center]=1-(Wc2-Wc1)*INVPI;
			for(n=1;n<=center;n++)
				fir[center+n]=(sin(Wc1*n)-sin(Wc2*n))/(PI*n);
		break;
		default://LOWPASS
			fir[center]=Wc1*INVPI;
			for(n=1;n<=center;n++)
				fir[center+n]=sin(Wc1*n)/(PI*n);
		break;
	}
	switch(windowType)
	{
		case FIR_WIN_BARTLETT:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=1.0-(2.0*n)/(order+1);
		break;
		case FIR_WIN_VONHANN:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.5+0.5*cos((2*PI*n)/order);
		break;
		case FIR_WIN_HAMMING:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.54+0.46*cos((2*PI*n)/order);
		break;
		case FIR_WIN_BLACKMAN:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.42659071+0.49656062*cos((2*PI*n)/order)+0.07684867*cos((4*PI*n)/order);
		break;
		case FIR_WIN_BLACKMAN_HARRIS_3:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.44959+0.49364*cos((2*PI*n)/order)+0.05677*cos((4*PI*n)/order);
		break;
		case FIR_WIN_BLACKMAN_HARRIS_4:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.40217+0.49703*cos((2*PI*n)/order)+0.09892*cos((4*PI*n)/order)+0.00183*cos((6*PI*n)/order);
		break;
		case FIR_WIN_HARRIS_NUTALL_3:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.42323+0.49755*cos((2*PI*n)/order)+0.07922*cos((4*PI*n)/order);
		break;
		case FIR_WIN_HARRIS_NUTALL_4:
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n]*=0.35875+0.48829*cos((2*PI*n)/order)+0.14128*cos((4*PI*n)/order)+0.01168*cos((6*PI*n)/order);
		break;
		default://FIR_WIN_RECT
			for(n=1;n<=center;n++)
				fir[center-n]=fir[center+n];
		break;
	}
	return TRUE;
}
GaussMarkov::GaussMarkov(double stdev, double beta, double sample):
realScan(-1)
{
	init(stdev,beta,sample);
}
GaussMarkov::GaussMarkov():
realScan(-1)
{
	init(1,1,1);
}
double GaussMarkov::next(double sample) // generator
{
	if(dt!=sample)
	{
		dt=sample;
		c1=exp(-beta*dt);
		c2=stdev*sqrt(1-c1*c1);
	}
	return x=c1*x+c2*random_n();
}
double GaussMarkov::rand()
{
	return x=random_n(stdev);
}
void GaussMarkov::init(double newStdev, double newBeta, double sample, BOOL random)
{
	if(newStdev<0)
		newStdev=0;
	if(newBeta<0)
		newBeta=0;
	if(sample<=0)
		sample=1;
	stdev=newStdev;
	beta=newBeta;
	dt=sample;
	c1=exp(-beta*dt);
	c2=stdev*sqrt(1-c1*c1);
	x=(random)?random_n(stdev):0;
}
// Get The Parameters Of A Gauss-Markov Process From A Noise Signal,
// By Using The Least Squares Method After Linearization Of The
// Signal's Auto-Correlation Function Width Logarithms
// Return The Used Scan Length Or 0 In Case Of Error
int GaussMarkov::init(	const double *y, // y data samples
						const double *x, // x data samples, NULL if (dt) should be used							
						int count, // length of the signal samples
						int shift, // max shift for auto-correlation calculation
						double dt, // sample time if (x) doesn't exist and to use later in the process generator 
						int index, // start index for auto-correlation calculation
						int scan) // scan length for auto-correlation calculation
						
{
	int l,step,n;
	double coefs[2]={0,0},*R,*t_R,*lnR,*t_lnR;
	realScan=0;
	if(!y||count<=0)
		return 0;
	l=ABS(shift);
	if(l<2)
		return 0;
	if(!(R=buff_R.setSize(l))||!(t_R=buff_t_R.setSize(l)))
		return 0;
	if(dt<=0)
		dt=1;
	realScan=getAutoCorrelFunc(y,count,R,shift,index,scan);
	if(!realScan)
		return 0;
	n=0;
	for(int i=0;i<l;i++)
		if(R[i]>0)
			n++;
	if(!n||!(lnR=buff_lnR.setSize(n))||!(t_lnR=buff_t_lnR.setSize(n)))
		return 0;
	step=shift/l;
	n=0;
	if(x)
	{
		for(int i=0,j=0; i<l ;i++,j+=step)
		{
			t_R[i]=x[index+j]-x[index];
			if(R[i]>0)
			{
				lnR[n]=log(R[i]);
				t_lnR[n]=t_R[i];
				n++;
			}
		}
	}
	else
	{
		for(int i=0,j=0; i<l ;i++,j+=step)
		{
			t_R[i]=j*dt;
			if(R[i]>0)
			{
				lnR[n]=log(R[i]);
				t_lnR[n]=t_R[i];
				n++;
			}
		}
	}
	if(!getLSMPolyCoef(coefs,2,lnR,t_lnR,n))
		return 0;
	init(	exp(0.5*coefs[0]),	// a0=2ln(stdev)
			-coefs[1],			// a1=-beta	
			dt);
	return realScan;
}
// Get Polynomial Coefs By Using The Least Squares Method:
// SUM(((Yi-Y0) - SUM(Ck*(Xi-X0)^k))^2)=min =>derivate for all Cj
// i=0:N-1   k=0:M-1
// =>M equations:	SUM((Yi-Y0)*(Xi-X0)^j)=SUM(Ck * SUM((Xi-X0)^(k+j)))
//					i=0:N-1           k=0:M-1  i=0:N-1
// => B = A*C
// => C=inv(A)*B
BOOL getLSMPolyCoef(double *coefs, // C
					int count, // M
					const double *y, // y data
					const double *x, // x data
					int length, // N
					double x0, // X0,
					double y0, // Y0
					double threshold) // Matrix Singularity Factor
{ // todo: normalize the calculs with the number of input points!
	if(!y||!x||length<=0||count<=0||count>length)
		return FALSE;
	register int i,j,k,n,t;
	double tmp;
	Matrix A(count,count),Ainv(count,count),B(count,1),C(count,1);
	// Fill Up Matrix 'A':
	// Making 2*n sums instead of n^2, because:
	// 1 2 3 M
	// 2 3 M .
	// 3 M . .
	// M . . .
	i=0;j=0;n=1;k=0;
	for(;j<count-1;j++,n++,k++)
	{
		for(t=0,tmp=0;t<length;t++)
			tmp+=pown(x[t]-x0,k);
		for(t=0;t<n;t++)
			A.elems[i+t][j-t]=tmp;
	}
	for(;i<count;i++,n--,k++)
	{
		for(t=0,tmp=0;t<length;t++)
			tmp+=pown(x[t]-x0,k);
		for(t=0;t<n;t++)
			A.elems[i+t][j-t]=tmp;
	}
	// Fill Up Matrix 'B':
	for(i=0;i<count;i++)
	{
		for(t=0,tmp=0;t<length;t++)
			tmp+=(y[t]-y0)*pown(x[t]-x0,i);
		B.elems[i][0]=tmp;
	}
	if(!invMatrix(A.elems,Ainv.elems,count,threshold))
		return FALSE;
	mulMatrix(Ainv.elems,B.elems,C.elems,count,count,1);
	for(i=0;i<count;i++)
		coefs[i]=C.elems[i][0];
	return TRUE;
}
// Get Interpolation Matrix:
// y=SUM(Yi*MUL((X-Xj)/(Xi-Xj)))=SUM(Yi*SUM(Cik*(X-Xc)^k))
//   i=0:N-1,j=0:N-1,j!=i		        k=0:N-1
BOOL getInterTable( MATRIX c, // Matrix (NxN) Cij 
					int n, // N
					const double *x, // X0,X1,...,Xn-1
					double xc)
{
	int i,j,k,d;
	double dxr,dxc,*ci;
	if(!c||n<1)
		return FALSE;
	for(i=0;i<n;i++)
	{
		ci=c[i];
		ci[0]=1;
		for(j=0,d=0;j<n;j++)
		{
			if(i==j)
				continue;
			dxr=INV(x?(x[i]-x[j]):(i-j));
			dxc=xc-(x?x[j]:j);
			ci[d+1]=ci[d]*dxr;
			for(k=d;k>0;k--)
				ci[k]=(ci[k-1]+ci[k]*dxc)*dxr;
			ci[0]*=dxc*dxr;
			d++;
		}
	}
	return TRUE;
}
// Get Interpolation Coefficients:
// y=SUM(Yi*SUM(Cik*(0-Xc)^k))=SUM(Ak*(0-Xc)^k)
//	  i=0:N-1,k=0:N-1,
BOOL getInterCoef( 	double *coef, // A0,A1,...,An-1
					int n, // N
					const double *y, // Y0,Y1,...,Yn-1
					CMATRIX c) // Interpolation Table (NxN) Cij
{
	register int i,j;
	if(!(coef&&c&&y&&n>0))
		return FALSE;
	for(i=0;i<n;i++)
	{
		coef[i]=0;
		for(j=0;j<n;j++)
			coef[i]+=c[j][i]*y[j];
	}
	return TRUE;
}
// Get n Interpolation Coefs For y=f(x-x0)
BOOL getInterCoef( 	double *coef, // A0,A1,...,An-1
					int n,
					const double *y, // n Values For y
					const double *x, // n Values For x
					double x0)
{
	Matrix interTable;
	if(!interTable.setSize(n,n)||!getInterTable(interTable,n,x,x0))
		return FALSE;
	return getInterCoef(coef,n,y,interTable);
}
// Get n Taylor Coefs For y=f(x-x0)
BOOL getTaylorCoef( double *coef, // A0,A1,...,An-1
					int n,
					double (*dydx)(double,int), // Derivate k Function
					double x0)
{
	if(!(coef&&dydx))
		return FALSE;
	double f=1;
	for(int i=0;i<n;i++)
	{
		coef[i]=dydx(x0,i)/f;
		f*=(i+1);
	}
	return TRUE;
}
// Calculate Function By Its Polynom Coefficients
double funcValue(	double x,	
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double x0, // Interpolation Origin
					double k) // Divider k*(x-x0)
{
	if(!(coef&&n>0&&k))
		return 0;
	int i=n-1;
	double y=coef[i],ratio=k*(x-x0);
	while(i)
	{
		y*=ratio;
		i--;
		y+=coef[i];
	}
	return y;
}
// Calculate Integral By The Polynom Coefficients Of The Function
double integValue( 	double x,
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double y0, // Integration origin
					double x0,
					double k)
{
	if(!(coef&&n>0&&k))
		return 0;
	int i=n-1;
	double y=coef[i],ratio=k*(x-x0);
	while(i)
	{
		y*=i*ratio/(i+1);
		i--;
		y+=coef[i];
	}
	return y0+y*(x-x0)/k;
}
// Calculate Derivate By The Polynom Coefficients Of The Function
double derivValue(	double x,
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double x0,
					double k)
{
	if(!(coef&&n>1&&k))
		return 0;
	int i=n-1;
	double y=coef[i],ratio=k*(x-x0);
	while(i!=1)
	{
		y*=i*ratio/(i-1);
		i--;
		y+=coef[i];
	}
	return k*y;
}

Interpolator::Interpolator():data(NULL){}
int Interpolator::init(double *data, int dataSize, double start, double end, int interSize)
{
	if(!data||dataSize<2||start==end)
		return 0;
	index=-1;
	value=0;
	this->data=data;
	this->dataSize=dataSize;
	this->start=start;
	this->end=end;
	stepSize=(end-start)/(dataSize-1);
	stepSizeRec=INV(stepSize);
	if(interSize<2)
		interSize=2;
	else if(interSize>dataSize)
		interSize=dataSize;
	this->interSize=interSize;
	interCoef.setSize(interSize);
	if(!interTable.setSize(interSize,interSize))
		return 0;
	if(!getInterTable(interTable.elems,interSize))
		return 0;
	return interSize;
}
BOOL Interpolator::set(double x)
{
	if(!data)
		return FALSE;
	double k=(x-start)/(end-start);
	if(k>=0&&k<=1)
	{
		int newIndex=__int(k*(dataSize-1))-(interSize-1)/2;
		if(newIndex<0)
			newIndex=0;
		else if(newIndex>dataSize-interSize)
			newIndex=dataSize-interSize;
		if(index!=newIndex)
		{
			index=newIndex;
			x0=start+index*stepSize;
			for(int i=0;i<interSize;i++)
			{
				interCoef.elems[i]=0;
				for(int j=0;j<interSize;j++)
					interCoef.elems[i]+=interTable.elems[j][i]*data[index+j];
			}
		}
		value=funcValue(x,interCoef.elems,interSize,x0,stepSizeRec);
		return TRUE;
	}
	return FALSE;
}

}; // namespace Lio